﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Timers;
using System.IO;
using System.Diagnostics;

namespace SCARA_GUI
{
    public partial class form : Form
    {
        private System.Windows.Forms.Timer timer;
        private SCARAComm communicationToSCARA;

        public form()
        {
            InitializeComponent();
            communicationToSCARA = new SCARAComm();
        }

        private void Form1_Load(object sender, EventArgs e)
        {
            timer = new System.Windows.Forms.Timer();
            timer.Tick += new EventHandler(getRobotState);//到达时间的时候执行事件；
            timer.Interval = 333;//设置是执行一次（false）还是一直执行(true)；
        }

        public void getRobotState(object sender, EventArgs e)
        {
            try
            {
                double[] pos, vel, tq;
                string[] state;
                pos = communicationToSCARA.GetDrivePos();
                tbPosition1.Text = pos[0].ToString("F3");
                tbPosition2.Text = pos[1].ToString("F3");
                tbPosition3.Text = pos[2].ToString("F3");
                tbPosition4.Text = pos[3].ToString("F3");
                vel = communicationToSCARA.GetDriveVel();
                tbVelocity1.Text = vel[0].ToString("F2");
                tbVelocity2.Text = vel[1].ToString("F2");
                tbVelocity3.Text = vel[2].ToString("F2");
                tbVelocity4.Text = vel[3].ToString("F2");
                tq = communicationToSCARA.GetDriveTq();
                tbTorque1.Text = tq[0].ToString("F2");
                tbTorque2.Text = tq[1].ToString("F2");
                tbTorque3.Text = tq[2].ToString("F2");
                tbTorque4.Text = tq[3].ToString("F2");
                state = communicationToSCARA.GetDriveState();
                tbState1.Text = state[0];
                tbState2.Text = state[1];
                tbState3.Text = state[2];
                tbState4.Text = state[3];
                state = communicationToSCARA.GetProfileState();
                tbPfState1.Text = state[0];
                tbPfState2.Text = state[1];
                tbPfState3.Text = state[2];
                tbPfState4.Text = state[3];
            }
            catch (Exception err)
            {
                //MessageBox.Show(err.Message);
                timer.Enabled = false;
            }
     
        }

        /*
         * Connect the client to the local AMS router 
         */

        private void btConnect_Click(object sender, EventArgs e)
        {
            Int32 port;
            try
            {
                // check if port is a hex value
                if (tbPort.Text.StartsWith(("0x")) || tbPort.Text.StartsWith(("0X")))
                {
                    string sHexValue = tbPort.Text.Substring(2);
                    port = Int32.Parse(sHexValue, System.Globalization.NumberStyles.HexNumber);
                }
                // interpret as dec value
                else
                {
                    port = Int32.Parse(tbPort.Text);
                }
            }
            catch
            {
                MessageBox.Show("Invalid AMS NetId or Ams port");
                return;
            }

            try
            {
                communicationToSCARA.Connect(tbNetId.Text, port);
                lbOutput.Items.Add("Client port " + communicationToSCARA.clientPort + " opened");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
                timer.Enabled = true;//是否执行System.Timers.Timer.Elapsed事件；
            }
            catch
            {
                MessageBox.Show("Could not connect client");
            }
        }

        private void btEnable1_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.EnableDrive(1);
                lbOutput.Items.Add("Motor1 enabled!");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
            } 
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btDisable1_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.DisableDrive(1);
                lbOutput.Items.Add("Motor1 disabled!");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btClearFault1_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.ClearDriveFault(1);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btEnable2_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.EnableDrive(2);
                lbOutput.Items.Add("Motor2 enabled!");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;

            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btDisable2_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.DisableDrive(2);
                lbOutput.Items.Add("Motor2 disabled!");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btClearFault2_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.ClearDriveFault(2);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btEnable3_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.EnableDrive(3);
                lbOutput.Items.Add("Motor3 enabled!");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btDisable3_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.DisableDrive(3);
                lbOutput.Items.Add("Motor3 disabled!");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btClearFault3_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.ClearDriveFault(3);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btEnable4_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.EnableDrive(4);
                lbOutput.Items.Add("Motor4 enabled!");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btDisable4_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.DisableDrive(4);
                lbOutput.Items.Add("Motor3 disabled!");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btClearFault4_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.ClearDriveFault(4);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btJogPositive4_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.MoveDrive(4, true, 180, 360, 720);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btJogNegative4_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.MoveDrive(4, true, -180, 360, 720);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btStop4_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.StopDrive(4);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btJogPositive1_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.MoveDrive(1, true, 5, 10, 30);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btJogNegative1_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.MoveDrive(1, true, -5, 10, 30);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btStop1_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.StopDrive(1);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btJogPositive2_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.MoveDrive(2, true, 5, 20, 40);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btJogNegative2_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.MoveDrive(2, true, -5, 20, 40);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btStop2_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.StopDrive(2);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btJogPositive3_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.MoveDrive(3, true, 180, 360, 720);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btJogNegative3_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.MoveDrive(3, true, -180, 360, 720);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btStop3_Click(object sender, EventArgs e)
        {
            try
            {
                communicationToSCARA.StopDrive(3);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btConfirm1_Click(object sender, EventArgs e)
        {
            try
            {
                double pos = double.Parse(tbTgtPos1.Text);
                double vel = double.Parse(tbTgtVel1.Text);
                double acc = double.Parse(tbTgtAcc1.Text);
                lbOutput.Items.Add("Motor1 moving to " + pos.ToString() + "° at " + vel.ToString() + "°/s with acceleration" + acc.ToString() + "°/s^2.");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
                communicationToSCARA.MoveDrive(1, false, pos, vel, acc);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btConfirm2_Click(object sender, EventArgs e)
        {
            try
            {
                double pos = double.Parse(tbTgtPos2.Text);
                double vel = double.Parse(tbTgtVel2.Text);
                double acc = double.Parse(tbTgtAcc2.Text);
                lbOutput.Items.Add("Motor2 moving to " + pos.ToString() + "° at " + vel.ToString() + "°/s with acceleration" + acc.ToString() + "°/s^2.");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
                communicationToSCARA.MoveDrive(2, false, pos, vel, acc);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btConfirm3_Click(object sender, EventArgs e)
        {
            try
            {
                double pos = double.Parse(tbTgtPos3.Text);
                double vel = double.Parse(tbTgtVel3.Text);
                double acc = double.Parse(tbTgtAcc3.Text);
                lbOutput.Items.Add("Motor3 moving to " + pos.ToString() + "° at " + vel.ToString() + "°/s with acceleration" + acc.ToString() + "°/s^2.");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
                communicationToSCARA.MoveDrive(3, false, pos, vel, acc);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btConfirm4_Click(object sender, EventArgs e)
        {
            try
            {
                double pos = double.Parse(tbTgtPos4.Text);
                double vel = double.Parse(tbTgtVel4.Text);
                double acc = double.Parse(tbTgtAcc4.Text);
                lbOutput.Items.Add("Motor4 moving to " + pos.ToString() + "° at " + vel.ToString() + "°/s with acceleration" + acc.ToString() + "°/s^2.");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
                communicationToSCARA.MoveDrive(4, false, pos, vel, acc);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btBrowse_Click(object sender, EventArgs e)
        {
            string resultFile = "";

            OpenFileDialog openFileDialog = new OpenFileDialog();
            openFileDialog.InitialDirectory = "C:\\";
            openFileDialog.Filter = "All files (*.*)|*.*|txt files (*.txt)|*.txt";
            openFileDialog.FilterIndex = 2;
            openFileDialog.RestoreDirectory = true;
            if (openFileDialog.ShowDialog() == DialogResult.OK)
                resultFile = openFileDialog.FileName;
            tbFilePath.Text = resultFile;
        }

        private void btTransfer_Click(object sender, EventArgs e)
        {
            try
            {
                FileStream fs = new FileStream(tbFilePath.Text, FileMode.Open);
                StreamReader sr = new StreamReader(fs);
                string line;
                List<double>[] trajectory = new List<double>[4];
                for(int i=0;i<4;i++)
                {
                    trajectory[i] = new List<double>();
                }

                while ((line = sr.ReadLine()) != null)
                {
                    string[] bits = line.Split(',');
                    int ii = 0;
                    foreach (string bit in bits)
                    {
                        double value;
                        if (!double.TryParse(bit, out value))
                        {
                            Console.WriteLine("Bad value");
                        }
                        trajectory[ii].Add(value);
                        ii++;
                    }
                }

                sr.Close();
                fs.Close();

                double[] J1, J2, J3, J4;
                J1 = trajectory[0].ToArray();
                J2 = trajectory[1].ToArray();
                J3 = trajectory[2].ToArray();
                J4 = trajectory[3].ToArray();

                lbOutput.Items.Add("File name: " + tbFilePath.Text);
                lbOutput.Items.Add("Starting point: " + J1[0].ToString() + ", " + J2[0].ToString() + ", " + J3[0].ToString() + ", " + J4[0].ToString() + ".");
                tbTgtPos1.Text = J1[0].ToString();
                tbTgtPos2.Text = J2[0].ToString();
                tbTgtPos3.Text = J3[0].ToString();
                tbTgtPos4.Text = J4[0].ToString();
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;

                communicationToSCARA.InputTrajectory(1, J1);
                communicationToSCARA.InputTrajectory(2, J2);
                communicationToSCARA.InputTrajectory(3, J3);
                communicationToSCARA.InputTrajectory(4, J4);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }


        }

        private void btStartExecution_Click(object sender, EventArgs e)
        {
            try
            {
                bool isLoop = cbLoop.Checked;
                lbOutput.Items.Add("Execution Command Sent ");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
                communicationToSCARA.StartExecution(isLoop);
            }
            catch (Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }

        private void btAbort_Click(object sender, EventArgs e)
        {
            try
            { communicationToSCARA.AbortExecution();
                lbOutput.Items.Add("Trajectory execution aborted.");
                lbOutput.SelectedIndex = lbOutput.Items.Count - 1;
            }
            catch(Exception err)
            {
                MessageBox.Show(err.Message);
            }
        }
    }
}
